//
// Created by hoyin on 2022/3/6.
//

#ifndef AUST_RM_VISION_2022_CVUTILS_H
#define AUST_RM_VISION_2022_CVUTILS_H

#include <utility>
#include "MathUtils.h"

#include "opencv2/opencv.hpp"
#include "opencv2/plot.hpp"

using namespace cv;

void drawRotatedRect(const RotatedRect& rect, Mat& dst, const Scalar& color, int thickness=1, const Point2f& shift=Point2f(0, 0)) {
	Point2f* pts = new Point2f[4];
	rect.points(pts);
	for (int i = 0; i < 4; ++i) {
		line(dst, Point2f(pts[i].x + shift.x, pts[i].y + shift.y), Point2f(pts[(i+1)%4].x + shift.x, pts[(i+1)%4].y + shift.y), color, thickness);
	}
}

Point2f getCrossPoint(vector<Point2f> lineA, vector<Point2f> lineB) {
	float xa1 = lineA[0].x, ya1 = lineA[0].y, xa2 = lineA[1].x, ya2 = lineA[1].y;
	float xb1 = lineB[0].x, yb1 = lineB[0].y, xb2 = lineB[1].x, yb2 = lineB[1].y;
	double ka, kb;
	ka = (ya2 - ya1) / (xa2 - xa1);
	kb = (yb2 - yb1) / (xb2 - xb1);
//	ka = (lineA[3] - lineA[1]) / (lineA[2] - lineA[0]);
//	kb = (lineB[3] - lineB[1]) / (lineB[2] - lineB[0]);

	Point2f crossPoint;
	crossPoint.x = float ((ka*xa1 - ya1 - kb*xb1 + yb1) / (ka - kb));
	crossPoint.y = float ((ka*kb*(xa1 - xb1) + ka*yb1 - kb*ya1) / (ka - kb));
	return crossPoint;
}


double getAngleOf2Vectors(const Vec2d& vec1, const Vec2d& vec2) {
	double cos_th = (vec1.dot(vec2)) / (sqrt(vec1.dot(vec1)) * sqrt(vec2.dot(vec2)));
	return acos(cos_th);
}

double getQuadrant(Vec2d& vec) {
	vector<double> vector;
	vector.push_back(vec[0]);
	vector.push_back(vec[1]);
	return mu::getQuadrant(vector);
}

void getCenterOfRect(const RotatedRect& rect, Point2f& dst) {
	auto* pts = new Point2f[4];
	rect.points(pts);
	vector<Point2f> lineA, lineB;
	lineA.push_back(pts[0]);
	lineA.push_back(pts[2]);
	lineB.push_back(pts[1]);
	lineB.push_back(pts[3]);
	dst = getCrossPoint(lineA, lineB);
}

void showWaitKey(Mat& img, string winname) {
	imshow(winname, img);
	waitKey();
	destroyAllWindows();
}

void drawPlot(const vector<double>& data_x, const vector<double>& data_y, Mat& canvas, Scalar color, double minX=0, double maxX=0, double minY=0, double maxY=0) {
	Ptr<plot::Plot2d> plot = plot::Plot2d::create(data_x, data_y);
	plot->setNeedPlotLine(false);
	plot->setShowText(true);
	plot->setShowGrid(true);
	plot->setPlotBackgroundColor(Scalar(222, 222, 222));
	plot->setPlotLineColor(std::move(color));
	plot->setPlotGridColor(Scalar(200, 200, 200));
	plot->setPlotAxisColor(Scalar(5, 5, 5));
	plot->setPlotLineWidth(2);
	plot->setInvertOrientation(true);
	if (minX != maxX) {
		plot->setMinX(minX);
		plot->setMaxX(maxX);
	}
	if (minY != maxY) {
		plot->setMinY(minY);
		plot->setMaxY(maxY);
	}
	plot->setPlotTextColor(Scalar(20, 20,20));
	plot->render(canvas);
}

void convertPos_Image2Calc(const Point2d& src_img, const Point2d& offset, Point2d& dst_cal) {
	dst_cal.x = src_img.x - offset.x;
	dst_cal.y = offset.y - src_img.y;
}

void convertPos_Calc2Image(const Point2d& src_cal, const Point2d& offset, Point2d& dst_img) {
	dst_img.x = offset.x + src_cal.x;
	dst_img.y = offset.y - src_cal.y;
}

#endif //AUST_RM_VISION_2022_CVUTILS_H
